#include "usr_inc.h"

/* 定义对准模式 */
AimMode_t aimMode = AIM_DISABLE;

/* 底盘宽度 + 护身宽度 */
#define OFFSET_WIDTH (0.315 + 0.05)

/**
 * 全场坐标系:
 *              x
 *              ^
 *              |
 *              |
 *              |
 * y <----------o
 */

/* 篮筐中心坐标 */
#define HOOP_X_POS (15.0 - 0.975)
#define HOOP_Y_POS (4.0)

/* 底盘初始化坐标 */
#define INIT_X_POS (0.0 + OFFSET_WIDTH)
#define INIT_Y_POS (8.0 - OFFSET_WIDTH)

void StartAimUpdate(void *argument)
{
    for (;;)
    {
        AimMode_t mode;
        xQueueReceive(aimModeQueueHandle, &mode, portMAX_DELAY);

        aimMode = mode;
        if (mode == AIM_DISABLE)
        {
            vTaskDelay(pdMS_TO_TICKS(100));
            float yaw = 0.0;
            xQueueSendToBack(chassisYawCmdQueueHandle, &yaw, 1);
        }
    }
}

void StartAimExe(void *argument)
{
    /* 记录对准篮筐转角的溢出信息 */
    float lastHoopYaw = 0.0;
    int16_t ovHoop = 0;

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 10; ;)
    {
        if (aimMode == AIM_HOOP)
        {
            /* 获取雷达信息 */
            LocaterMsg_t radarMsg;
            xQueuePeek(radarLocHandle, &radarMsg, portMAX_DELAY);
            RADAR_ConvertToCenter(&radarMsg);

            /* 计算对准篮筐所需的信息 */
            float dis, yaw;
            AIM_GetHoopInfo(&radarMsg, &dis, &yaw);

            /* 判断转角是否溢出 */
            float temp = yaw - lastHoopYaw;
            if (fabsf(temp) > 180.0)
            {
                temp < 0.0 ? ovHoop++ : ovHoop--;
            }
            lastHoopYaw = yaw;

            /* 更新转角 */
            yaw = yaw + ovHoop * 360.0;

            /* 发送射球信息和转角 */
            if (dis <= 8.0)
            {
                ShootArgsMsg_t args;
                args.d = dis;
                args.h = 2.43;
                xQueueSendToBack(shootArgsQueueHandle, &args, 1);
            }
            xQueueSendToBack(chassisYawCmdQueueHandle, &yaw, 1);
        }
        else if (aimMode == AIM_R2)
        {
            /* 获取雷达信息 */
            LocaterMsg_t radarMsg;
            xQueuePeek(radarLocHandle, &radarMsg, portMAX_DELAY);
            RADAR_ConvertToCenter(&radarMsg);

            /* TODO: 获取R2底盘中心在全场坐标系下的位置, 注意单位和坐标系转换 */
            PointMsg_t r2Msg = {0};

            /* 计算对准R2所需的信息 */
            float dis, yaw;
            AIM_GetR2Info(&radarMsg, &r2Msg, &dis, &yaw);

            /* 发送射球信息和转角 */
            ShootArgsMsg_t args;
            args.d = dis;
            args.h = R2_HEIGHT;
            xQueueSendToBack(shootArgsQueueHandle, &args, 1);
            xQueueSendToBack(chassisYawCmdQueueHandle, &yaw, 1);
        }

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

/**
 * @brief 计算对准篮筐所需的信息
 */
void AIM_GetHoopInfo(const LocaterMsg_t *locMsg, float *distance, float *yaw)
{
    /* 底盘中心到篮筐中心的距离 */
    *distance = hypotf(HOOP_X_POS - (locMsg->posX + INIT_X_POS), HOOP_Y_POS - (locMsg->posY + INIT_Y_POS));

    /* 底盘对准篮筐所需要自转的角度 */
    *yaw = ALGO_Rad2Deg(atan2f(HOOP_Y_POS - (locMsg->posY + INIT_Y_POS), HOOP_X_POS - (locMsg->posX + INIT_X_POS)));
}

/**
 * @brief 计算对准R2所需的信息
 */
void AIM_GetR2Info(const LocaterMsg_t *locMsg, const PointMsg_t *r2Msg, float *distance, float *yaw)
{
    /* 底盘中心到R2中心的距离 */
    *distance = hypotf(r2Msg->posX - (locMsg->posX + INIT_X_POS), r2Msg->posY - (locMsg->posY + INIT_Y_POS));

    /* 底盘对准R2所需要自转的角度 */
    *yaw = ALGO_Rad2Deg(atan2f(r2Msg->posY - (locMsg->posY + INIT_Y_POS), r2Msg->posX - (locMsg->posX + INIT_X_POS)));
}
